/*
    Copyright 2004-2005 (c) by Aitor Viana Sanchez,
    University of Alcala,
    Computer Engineering Department.

    For further information, please visit http://atc1.aut.uah.es

    This software is provided under the terms of the GNU General Public v2
    Licence. A full copy of the GNU GPL is provided in the file COPYING
    found in the development root of POSIX-RTEMS.
*/

#include <edroombp_swr/posix/rtems_4_6/edroombp.h>
#include <public/debug_primitives.h>

//------------------------------------------------------------------------------

#define NSECS_PER_SEC 1000000000
#define CLICKS_PER_SEC	1000000
#define CLICKS_PER_USEC	1
typedef uint64_t OS_Time_t;

/**
 * \todo Documentar esta macro
 */
#define _POSIX_timespec_to_ticks(_timespec, _ticks) \
    do { \
        _ticks = (OS_Time_t)((OS_Time_t)_timespec->tv_sec * CLICKS_PER_SEC) + \
                 _timespec->tv_nsec/(1000) * CLICKS_PER_USEC; \
    }while(0)

/**
 * \todo Documentar esta macro
 */
#define _POSIX_ticks_to_timespec(_timespec, _ticks) \
    do { \
        _timespec->tv_sec = _ticks / (CLICKS_PER_SEC * 1); \
        _ticks -= _timespec->tv_sec * (CLICKS_PER_SEC * 1); \
        _timespec->tv_nsec = _ticks  * 1000000000 / (CLICKS_PER_SEC); \
    }while(0) \


/* convenience functions */
#define _POSIX_timespec_normalize(t) {\
    if ((t)->tv_nsec >= NSECS_PER_SEC) { \
     (t)->tv_nsec -= NSECS_PER_SEC; \
       (t)->tv_sec++; \
   } else if ((int)(t)->tv_nsec < 0) { \
       (t)->tv_nsec += NSECS_PER_SEC; \
       (t)->tv_sec--; \
   } \
}

#define _POSIX_timespec_add(t1, t2) do { \
 (t1)->tv_nsec += (t2)->tv_nsec;  \
 (t1)->tv_sec += (t2)->tv_sec; \
    _POSIX_timespec_normalize(t1);\
} while (0)

#define _POSIX_timespec_sub(t1, t2) do { \
  (t1)->tv_nsec -= (t2)->tv_nsec;  \
 (t1)->tv_sec -= (t2)->tv_sec; \
    _POSIX_timespec_normalize(t1);\
} while (0)

#define _POSIX_timespec_add_ns(t,n) do { \
  (t)->tv_nsec += (n);  \
    _POSIX_timespec_normalize(t); \
} while (0)

#define _POSIX_timespec_nz(t) ((t)->tv_sec != 0 || (t)->tv_nsec != 0)

#define _POSIX_timespec_lt(t1, t2) ((t1)->tv_sec < (t2)->tv_sec || ((t1)->tv_sec == (t2)->tv_sec && (t1)->tv_nsec < (t2)->tv_nsec))

#define _POSIX_timespec_gt(t1, t2) (_POSIX_timespec_lt(t2, t1))

#define _POSIX_timespec_ge(t1, t2) (!_POSIX_timespec_lt(t1, t2))

#define _POSIX_timespec_le(t1, t2) (!_POSIX_timespec_gt(t1, t2))

#define _POSIX_timespec_eq(t1, t2) ((t1)->tv_sec == (t2)->tv_sec && (t1)->tv_nsec == (t2)->tv_nsec)

//------------------------------------------------------------------------------

static pthread_key_t key;
static pthread_t thread_ids[50];
static int n_threads;
static timespec start_time;

//******************************************************
//****************  Pr_Kernel **************************
//******************************************************

Pr_Kernel::Pr_Kernel()
{
    int status;

    n_threads = 0;
    status = pthread_key_create(&key, (void(*)(void*))NULL);
    assert(!status);
}

void Pr_Kernel::Start()
{

    clock_gettime(CLOCK_REALTIME, &start_time);
    for(int i = 0; i < n_threads; ++i)
        pthread_join(thread_ids[i], NULL);
}

//******************************************************
//****************  Pr_Task ****************************
//******************************************************


//****************  CONSTRUCTORS ***********************

#ifdef _EDROOM_IS_DEBUG_

Pr_Task::Pr_Task(void (*_taskCode)(void), /*  Task IP */
                const char *_p_name,              /*  Task name   */
                TEDROOMPriority _priority,     /*  task priority   */
                unsigned _stackSize):name(_p_name),        /*  stack size  */
                                semSend("TaskSem Send",0), semReceive("TaskSem recv",0)
{

#else

Pr_Task::Pr_Task(void (*_taskCode)(void), /*  Task IP */
                TEDROOMPriority _priority,     /*  task priority   */
                unsigned _stackSize):        /*  stack size  */
                                semSend(0), semReceive(0)
{

#endif

    int policy,status;
    struct sched_param param;

    /*  Stablish the task instruction pointer   */
    this->TaskIP = _taskCode;

    /*  Init the task priorities    */
    priorityMsg = priorityTmp = _priority;

    /*  Init the default attr thread values */
    status = pthread_attr_init(&attr);
    assert(!status);

    status = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    assert(!status);

    policy = SCHED_FIFO;
    param.sched_priority = GETPRIORITY(EDROOMprioURGENT);

    status = pthread_setschedparam(pthread_self(), policy, &param);
    assert(!status);
    status = pthread_getschedparam(pthread_self(), &policy, &param);
    assert(!status);

    /*  Set the scheduling policy of the thread */
    status = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    assert(!status);

    /*  Stablish the thread stack size  */
    status = pthread_attr_setstacksize(&attr, _stackSize);
    assert(!status);

    /*  set the priority; others are unchanged */
    param.sched_priority = GETPRIORITY(_priority);

    /*  setting the new scheduling param */
    status = pthread_attr_setschedparam(&attr, &param);
    assert(!status);

    /*  Create the thread   */
    status = pthread_create(&taskID, &attr, TaskFunction, (void *)this);
    assert(!status);

    thread_ids[n_threads++] = taskID;

}

void *Pr_Task::TaskFunction(void *arg)
{
    int status;
    Pr_Task *mythis = (Pr_Task*)arg;

    status = pthread_setspecific(key, (const void *)mythis);
    assert(!status);

    mythis->TaskIP();

    return NULL;
}

void Pr_Task::SetPriority(TEDROOMPriority _priority)
{
	ASSERT(_priority >= CONFIG_EDROOMBP_SWR_EDROOM_MIN_PRIORITY);
	ASSERT(_priority >= CONFIG_EDROOMBP_SWR_EDROOM_MAX_PRIORITY);

    priorityMsg = _priority;

    if (priorityTmp != priorityMsg) 
    {
        priorityTmp = priorityMsg;
        ChangePriority(_priority);
    }
}

void Pr_Task::SetMaxPrioTmp(TEDROOMPriority _priority)
{
    /*
     * We only change the priority if the new priority is greater than
     * (less value) the priorityTmp value
     */
    if (_priority < priorityTmp) 
    {
        priorityTmp = _priority;
        ChangePriority(_priority);
    }
}

void Pr_Task::SetMaxPrioMsg(TEDROOMPriority _priority)
{
    if (_priority < priorityMsg) 
    {
        priorityMsg = _priority;
        if (priorityMsg < priorityTmp) 
        {
            priorityTmp = priorityMsg;
            ChangePriority(_priority);
        }
    }
}

void Pr_Task::RestorePrioMsg(void)
{

    if (priorityTmp != priorityMsg) 
    {
        priorityTmp = priorityMsg;

        ChangePriority(priorityMsg);
    }
}

TEDROOMPriority Pr_Task::GetTaskPrio()
{
    struct sched_param param;
    int policy;

    pthread_getschedparam(taskID, &policy, &param);

    return (EDROOMPriority)param.sched_priority;
}

void Pr_Task::ChangePriority(TEDROOMPriority _priority)
{
    struct sched_param param;
    int policy;

    /*  We do not care about the task sched parameters */
    pthread_getschedparam(taskID, &policy, &param);

    /*  Sets the task priority  */
    param.sched_priority = GETPRIORITY(_priority);
    pthread_setschedparam(taskID, policy, &param);
}

//******************************************************
//****************  Pr_Time ****************************
//******************************************************

//****************  CONSTRUCTORS ***********************

Pr_Time::Pr_Time()
{

    time.tv_sec = 0;
    time.tv_nsec = 0;
}

Pr_Time::Pr_Time(const Pr_Time &_time)
{
    time.tv_nsec = _time.time.tv_nsec;
    time.tv_sec = _time.time.tv_sec;
}

Pr_Time::Pr_Time(float _seconds)
{
    float secs = _seconds;

    time.tv_sec = time.tv_nsec = 0;

    if(_seconds) {
        time.tv_sec = (time_t)secs;
        secs -= time.tv_sec;
        time.tv_nsec = (unsigned long)(secs * 1000000000);

    }

}

Pr_Time::Pr_Time(struct timespec _time)
{
    unsigned long nsec;
    time_t sec;

    sec = (time_t)(_time.tv_nsec / 1000000000);

    time.tv_sec = _time.tv_sec + sec;
    nsec = _time.tv_nsec - (sec * 1000000000);
    time.tv_nsec = nsec;
}


Pr_Time::Pr_Time(Pr_Time::unit _units, uint32_t _measure)
{
    time.tv_sec = time.tv_nsec = 0;

    if (_measure) 
    {
        switch (_units) 
        {
                                case ticks:
                                _POSIX_ticks_to_timespec((&time), _measure);
                                break;
                                case micros:
                                time.tv_nsec = _measure * 1000;
                                _POSIX_timespec_normalize((&time));
                                break;
                                case millis:
                                time.tv_nsec = _measure * 1000000;
                                _POSIX_timespec_normalize((&time));
                                break;
                                case secns:
                                time.tv_sec = _measure;
                                break;
                                case mints:
                                time.tv_sec = _measure * 60;
                                break;
                }
        }
}

//**************** MODIFIYING METHODS **********************

void Pr_Time::GetTime(void)
{
    struct timespec timeval;

    if (clock_gettime(CLOCK_REALTIME, &timeval) == 0)
    {
        time.tv_nsec = timeval.tv_nsec - start_time.tv_nsec;
        time.tv_sec = timeval.tv_sec - start_time.tv_sec;
    }

}

EDROOMClockTicksType Pr_Time::GetTicks()
{
    struct timespec current_time;
    EDROOMClockTicksType ticks = 0;

    if (clock_gettime(CLOCK_REALTIME, &current_time) == 0)
    {
        struct timespec *p = &current_time;
        _POSIX_timespec_to_ticks(p, ticks);
    }

    return ticks;
}

//**********   PR_TIME OPERATORS  *********************
void Pr_Time::Add(float seconds)
{
    struct timespec add;
    add.tv_sec = (time_t)seconds;
    add.tv_nsec = (long)((seconds - add.tv_sec) * NSECS_PER_SEC);

    _POSIX_timespec_add(&time, &add);
}



//**********   OPERATORS OVERLOAD  *********************

Pr_Time& Pr_Time::operator+=(const Pr_Time &_time)
{

    _POSIX_timespec_add(&time, &(_time.time));

    return *this;
}

Pr_Time& Pr_Time::operator-=(const Pr_Time &_time)
{
    if (_time.time.tv_sec <= time.tv_sec)
        {
                time.tv_sec -= _time.time.tv_sec;
                if (_time.time.tv_nsec > time.tv_nsec)
                {
                        if (time.tv_sec)
                        {
                                time.tv_sec--;
                                time.tv_nsec += 1000000000 - _time.time.tv_nsec;
                        }
                        else
                                time.tv_sec = time.tv_nsec = 0;
                }
                else
                        time.tv_nsec -= _time.time.tv_nsec;
        }
        else
                time.tv_sec = time.tv_nsec = 0;

//  _POSIX_timespec_sub(&time, &(_time.time));

    return *this;
}

Pr_Time& Pr_Time::operator=(const Pr_Time &_time)
{
    time.tv_nsec = _time.time.tv_nsec;
    time.tv_sec = _time.time.tv_sec;

    return *this;
}

int Pr_Time::operator==(const Pr_Time &_time)
{
    return (_POSIX_timespec_eq(&time, &(_time.time)));

}

int Pr_Time::operator!=(const Pr_Time &_time)
{
    return (!_POSIX_timespec_eq(&time, &(_time.time)));

}

int Pr_Time::operator>(const Pr_Time &_time)
{
    return (_POSIX_timespec_gt(&time, &(_time.time)));

}

int Pr_Time::operator<(const Pr_Time &_time)
{
    return (_POSIX_timespec_lt(&time, &(_time.time)));
}

int Pr_Time::operator>=(const Pr_Time &_time)
{
    return (_POSIX_timespec_ge(&time, &(_time.time)));
}

int Pr_Time::operator<=(const Pr_Time &_time)
{
    return (_POSIX_timespec_le(&time, &(_time.time)));
}

//*******   CONVERSION METHODS TO STANDARD UNITS   ******

float Pr_Time::Mints() const
{
    clock_t nticks;

    nticks = time.tv_sec * CLICKS_PER_SEC;

    if (time.tv_nsec > 1000)
        nticks = nticks + (time.tv_nsec / 1000) * CLICKS_PER_USEC;

    return (float)(uint32_t)nticks / (CLICKS_PER_SEC * 60);
}

float Pr_Time::Secns() const
{
    float aux;

    aux = time.tv_sec;// + (float)time.tv_nsec / 1000000000;

    return (float)(aux);
}

float Pr_Time::Millis() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000.0;

    return(aux * 1000);

}

float Pr_Time::Micros() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000;


    return (float)(aux * 1000000);
}

float Pr_Time::Ticks() const
{
    float aux;

    aux = time.tv_sec + (float)time.tv_nsec / 1000000000;


    return (float)(aux * CLICKS_PER_SEC);
}

void Pr_DelayIn(const Pr_Time &_interval)
{
    struct timespec time = _interval.time;

    nanosleep(&time, (struct timespec*)NULL);

}

void Pr_DelayAt( const Pr_Time &_time)
{
    struct timespec time;
    float sec1, sec2;

    clock_gettime(CLOCK_REALTIME, &time);
    time.tv_sec -= start_time.tv_sec;
    time.tv_nsec -= start_time.tv_nsec;

    sec1 = time.tv_sec + (float)time.tv_nsec / 1000000000;
    sec2 = _time.time.tv_sec + (float)_time.time.tv_nsec / 1000000000;

    if(sec2 > sec1) {

        sec2 -= sec1;

        time.tv_sec = (time_t)sec2;
        time.tv_nsec = (unsigned long)((sec2 - time.tv_sec) * 1000000000);

    }
    else
    {
        return;     // zero delay
    }


    nanosleep(&time, (struct timespec*)NULL);
}

//********************************************************
//********************  Pr_Semaphore  ********************
//********************************************************

Pr_Semaphore::Pr_Semaphore(unsigned int _value)
{
}


//********************************************************
//********************  Pr_SemaphoreBin  *****************
//********************************************************


#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreBin::Pr_SemaphoreBin(char *p_name, uint32_t _value) :
 Pr_Semaphore(_value), name(p_name)

#else
Pr_SemaphoreBin::Pr_SemaphoreBin(uint32_t _value) : Pr_Semaphore(_value)
#endif

{
    int status;

    status = sem_init(&sem, 0, _value);
    assert(!status);
}

void Pr_SemaphoreBin::Signal()
{
    sem_post(&sem);
}

void Pr_SemaphoreBin::Wait()
{
    sem_wait(&sem);
}

int Pr_SemaphoreBin::WaitCond()
{
    int retval = 0;

    if (sem_trywait(&sem) == 0)
        retval = 1;

    return retval;          /*  OK, return 1    */
}

bool Pr_SemaphoreBin::WaitTimed(const Pr_Time &_waittime)
{
    int status;
    struct timespec time, current_time;

    clock_gettime(CLOCK_REALTIME, &current_time);

    time.tv_sec = _waittime.time.tv_sec + current_time.tv_sec;
    time.tv_sec += (int)((_waittime.time.tv_nsec + current_time.tv_nsec) / 1000000000);

    // This must be an error because the unique value accepted in the
    // tv_nsec field is the zero one
    //time.tv_nsec = (long)((_waittime.time.tv_nsec + current_time.tv_nsec) & 0xFFFFFFFF);
    time.tv_nsec = 0x0;

    status = sem_timedwait (&sem, &time);

    return (status == 0);
}

//********************************************************
//********************  Pr_SemaphoreRec  *****************
//********************************************************

#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreRec::Pr_SemaphoreRec(const char *p_name) : name(p_name)
#else
Pr_SemaphoreRec::Pr_SemaphoreRec()
#endif
{
    pthread_mutexattr_init(&attr);
    //pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL);
    pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&mutex, &attr);
}

#ifdef _EDROOM_IS_DEBUG_
Pr_SemaphoreRec::Pr_SemaphoreRec(const char *p_name, int32_t prioceiling) : name(p_name)
#else
Pr_SemaphoreRec::Pr_SemaphoreRec(int32_t prioceiling)
#endif
{
    int priority_ceiling;

    pthread_mutexattr_init(&attr);
    //pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL);
    pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_PROTECT);

    /*
     * The priority in pthread_mutex with priority ceiling protocol
     * varies from 0 to 127 means 127 the higher priority. In EDROOM
     * the 0 priority value is the higher priority so we need to make a
     * change
     */
    priority_ceiling = EDROOM_PRIO_MINIMUM - prioceiling;
    priority_ceiling = (priority_ceiling < 0) ? 0 : priority_ceiling;

    pthread_mutexattr_setprioceiling (&attr, priority_ceiling);
    pthread_mutex_init(&mutex, &attr);
}

void Pr_SemaphoreRec::Signal()
{
    pthread_mutex_unlock(&mutex);
}

void Pr_SemaphoreRec::Wait()
{
    pthread_mutex_lock(&mutex);
}

int Pr_SemaphoreRec::WaitCond()
{
    if (pthread_mutex_trylock(&mutex) == 0)
        return 1;
    else
        return 0;
}


void Pr_Send(Pr_Task &_task, void *_p_data)
{
    _task.p_data = _p_data;
    _task.semReceive.Signal();
    _task.semSend.Wait();
}

void Pr_Receive(void * _p_data,unsigned _datalength)
{
    Pr_Task *receiver;

    receiver = (Pr_Task*)pthread_getspecific(key);

    receiver->semReceive.Wait();

    uint8_t *p_dataAux, *p_dataAuxReceiver;

    p_dataAux = (uint8_t *)_p_data;
    p_dataAuxReceiver = (uint8_t *)(receiver->p_data);

    for (unsigned int i = 0; i < _datalength; ++i)
        *(p_dataAux + i) = *(p_dataAuxReceiver + i);


    receiver->semSend.Signal();
}

//********************************************************
//********************  Pr_IRQManager ********************
//********************************************************
void Pr_IRQManager::DisableAllIRQs(void) 
{
	/* Nothing to do */
}

void Pr_IRQManager::ApplyCurrentIRQMask(void) 
{
	/* Nothing to do */
}

void Pr_IRQManager::InstallIRQHandler(Pr_IRQHandler handler,
		uint8_t IRQLevel, uint8_t IRQVectorNumber )
{

	intr_capture(IRQVectorNumber, (void*)handler);

}

void Pr_IRQManager::DeinstallIRQHandler(unsigned char IRQLevel,
		unsigned char IRQVectorNumber) 
{

	intr_restore (IRQVectorNumber);
}

void Pr_IRQManager::DisableIRQ(uint32_t IRQVectorNumber)
{
	//intr_lock (IRQVectorNumber);
}

void Pr_IRQManager::EnableIRQ(uint32_t IRQVectorNumber)
{
//	intr_unlock (IRQVectorNumber);
}

//********************************************************
//********************  Pr_IRQEvent ********************
//********************************************************

#ifdef _EDROOM_IS_DEBUG_

Pr_IRQEvent::Pr_IRQEvent(const char * pname,unsigned char IRQ) :
                        eventSem("Event Semaphore", 0), // init the semaphore
                        name(pname)                     // the Event name
{
}

#else

Pr_IRQEvent::Pr_IRQEvent(unsigned char IRQ) :
                        eventSem(0)    // init the semaphore
{}

#endif

void Pr_IRQEvent::Signal()
{
    eventSem.Signal();
}

void Pr_IRQEvent::Wait()
{
    eventSem.Wait();
}

bool Pr_IRQEvent::WaitTimed(Pr_Time _time)
{

    return eventSem.WaitTimed(_time);
}

bool Pr_IRQEvent::WaitCond()
{

    return eventSem.WaitCond();
}

//********************************************************
//********************  Pr_Trace  ************************
//********************************************************
#if 0
#ifdef CONFIG_EDROOMBP_SWR_TRACE_METHODS
Pr_Trace Pr_deb ("EDROOM-trace", 2);
Pr_Trace Pr_err ("EDROOM-error", 3);

Pr_Trace::Pr_Trace (void) {
	/* log trace events at level 2 */
	open (NULL, 2);
}

Pr_Trace::Pr_Trace (const char * str) {
	open (str, 2);
}

Pr_Trace::Pr_Trace (const char * str, unsigned level) {
	open (str, level);
}

void Pr_Trace::open (const char * str, unsigned level) {
	int ret;
	trace_id_t dummy;

	/* create the trace log */
	ret = posix_trace_create (0, NULL, &dummy);
	ASSERT (ret == 0);

	/* create a new event type */
	ret = posix_trace_eventid_open_with_level (str, &evid, level);
	ASSERT (ret == 0);
}

void Pr_Trace::close (void) {
	/* FIXME -- have we got to call to a posix_trace_close primitive ? */
}

void Pr_Trace::trace (const uint64_t & val) {
	char buff[16];
	unsigned int nr;

	/* FIXME -- printf hasn't got long long conversions */
	nr = sprintf (buff, "%u", (uint32_t) val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const uint32_t & val) {
	char buff[16];
	unsigned int nr;

	nr = sprintf (buff, "%u", val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const int32_t & val) {
	char buff[16];
	unsigned int nr;

	nr = sprintf (buff, "%d", val);
	posix_trace_event (evid, (void *) buff, nr);
}

void Pr_Trace::trace (const float & val) {
	char buff[16];
	unsigned int nr;

	/* FIXME -- printf hasn't float conversion */
	nr = sprintf (buff, "%d", (int32_t) val);
	posix_trace_event (evid, (void *) buff, nr);
}

Pr_Trace & Pr_Trace::operator << (const char * str) {
	posix_trace_event (evid, (void *) str, strlen (str));
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint64_t & val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint32_t val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (int32_t & val) {
	trace (val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint16_t & val) {
	trace ((uint32_t) val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (uint8_t & val) {
	trace ((uint32_t) val);
	return *this;
}

Pr_Trace & Pr_Trace::operator << (float & val) {
	trace (val);
	return *this;
}
#endif

#endif
